
// #include <geographic_msgs/GeoPoseStamped.h>
// #include <geometry_msgs/TwistStamped.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cstring>
#include <sstream>
#include <thread>
#include <cstdint> // For standard integer types
#include <cmath>   // For math functions// For SetMode service header

// #include <rclcpp/rclcpp.hpp>
// #include <sensor_msgs/msg/nav_sat_fix.hpp>
// #include <sensor_msgs/msg/imu.hpp>
// #include <geometry_msgs/msg/pose_stamped.hpp>
// #include <geographic_msgs/msg/geo_pose_stamped.hpp>
// #include <mavros_msgs/msg/vfr_hud.hpp>
// #include <mavros_msgs/srv/set_mode.hpp>
// #include <functional>
// #include <memory>
// #include <thread>
// #include <mutex>
// Define UDP parameters
#define UDP_TARGET_IP "192.168.16.190"     // Single-target IP address
#define UDP_PORT 8888                       // UDP data packet send port
#define UDP_RECEIVE_PORT 8899               // UDP data packet receive port



#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geographic_msgs/msg/geo_pose_stamped.hpp>
#include <mavros_msgs/msg/vfr_hud.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include <functional>
#include <memory>
#include <thread>
#include <mutex>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>

struct FCUCMD {
    double RefLatitude_deg;
    double RefLongitude_deg;
    double RefHeight_meter;
    double RefHdg_deg;
    double RefAirSpd_mps;
    double RefGndSpd_mps;
} __attribute__((packed));
struct uav_pos_raw {
    uint8_t seq;
    uint16_t year;
    uint8_t month;
    uint8_t day;
    uint8_t hour;
    uint8_t minute;
    uint16_t sec;
    int32_t latitude;
    int32_t longitude;
    int32_t altitude;
    int16_t pres;
    int16_t air_vel;
    int16_t ground_vel;
    int16_t roll;
    int16_t pitch;
    int16_t yaw;
    uint16_t rpm;
    uint16_t board_vol;
    uint8_t fs1;
    uint8_t fs2;
    uint8_t bd1;
    uint8_t bd2;
    uint8_t reserved[8];
    uint8_t back_type;
    uint8_t back_subtype;
    uint8_t back_verify;
} __attribute__((packed));



std::mutex udp_mutex;  // Mutex for shared data protection
class UavUdpComm : public rclcpp::Node {
public:
    UavUdpComm() : Node("uav_udp_comm") {
                 RCLCPP_INFO(this->get_logger(), "初始化节点");
        // 订阅GPS数据
        gps_subscriber_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            "/drone_0/global_position/raw/fix", rclcpp::QoS(10).best_effort(),
            std::bind(&UavUdpComm::gps_callback, this, std::placeholders::_1));
        // 订阅IMU数据
        imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/drone_0/imu/data",  rclcpp::QoS(10).best_effort(),
            std::bind(&UavUdpComm::imu_callback, this, std::placeholders::_1));
        // 发布地理位置
        pose_publisher_ = this->create_publisher<geographic_msgs::msg::GeoPoseStamped>(
            "/drone_0/setpoint_position/global", 10);
        setup_udp_communication();
        // 设置模式的服务客户端
        set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>("/drone_0/set_mode");

        // 启动UDP通信线程
        receive_thread_ = std::thread(&UavUdpComm::udp_receive_thread, this);

        send_thread_ = std::thread(&UavUdpComm::udp_send_thread, this);
    }

    ~UavUdpComm() {
        if (receive_thread_.joinable()) {
            receive_thread_.join();
        }
        if (send_thread_.joinable()) {
            send_thread_.join();
        }
    }

private:
    bool setOffboardMode() {
        auto set_mode_client = this->create_client<mavros_msgs::srv::SetMode>("/drone_0/set_mode");
        auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
        request->custom_mode = "OFFBOARD";

        RCLCPP_INFO(this->get_logger(), "Attempting to set Offboard mode...");

        // 发送异步请求
        auto result_future = set_mode_client->async_send_request(request, [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture future) {
            if (future.get()->mode_sent) {
                RCLCPP_INFO(this->get_logger(), "Offboard mode set successfully.");
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to set Offboard mode.");
            }
        });

        return true; // 返回true仅表示请求已发送
    }
    void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
        //  RCLCPP_INFO(this->get_logger(), "进入GPS回调函数");
        // 处理接收到的GPS数据
        std::lock_guard<std::mutex> lock(data_mutex);
        // 更新GPS数据
        data.latitude = static_cast<int32_t>(msg->latitude * (1LL << 31) / 90.0);
        data.longitude = static_cast<int32_t>(msg->longitude * (1LL << 31) / 180.0);
        data.altitude = static_cast<int32_t>(msg->altitude * (1LL << 31) / 35000.0);
        // RCLCPP_INFO(this->get_logger(), "Received GPS data: Lat %f, Lon %f", msg->latitude, msg->longitude);
    }
// IMU回调函数
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
        //  RCLCPP_INFO(this->get_logger(), "进入GPS回调函数");
        std::lock_guard<std::mutex> lock(data_mutex);
        // 更新IMU数据，并计算欧拉角
        double q_w = msg->orientation.w;
        double q_x = msg->orientation.x;
        double q_y = msg->orientation.y;
        double q_z = msg->orientation.z;
        double roll = atan2(2.0 * (q_w * q_x + q_y * q_z), 1.0 - 2.0 * (q_x * q_x + q_y * q_y));
        double pitch = asin(2.0 * (q_w * q_y - q_z * q_x));
        double yaw = atan2(2.0 * (q_w * q_z + q_x * q_y), 1.0 - 2.0 * (q_y * q_y + q_z * q_z));
        data.roll = static_cast<int16_t>(roll * 180.0 / M_PI * 100.0);
        data.pitch = static_cast<int16_t>(pitch * 180.0 / M_PI * 100.0);
        data.yaw = static_cast<int16_t>(yaw * 180.0 / M_PI * 100.0);
        // RCLCPP_INFO(this->get_logger(), "Received IMU data: Orientation [%f, %f, %f, %f]",
        //             msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
    }

    void udp_receive_thread() {
        FCUCMD cmd;
        bool offboard_set = false;  // Tracks whether Offboard mode has been enabled
        RCLCPP_INFO(this->get_logger(), "UDP Receive Thread Started");
        receiveSockfd = socket(AF_INET, SOCK_DGRAM, 0);
        if (receiveSockfd == -1) {
            RCLCPP_ERROR(this->get_logger(), "Failed to create UDP receive socket: %s", std::strerror(errno));
            return;
        }

        receiveAddr.sin_family = AF_INET;
        receiveAddr.sin_port = htons(UDP_RECEIVE_PORT);
        receiveAddr.sin_addr.s_addr = htonl(INADDR_ANY); // or specific interface
        if (bind(receiveSockfd, (struct sockaddr *)&receiveAddr, sizeof(receiveAddr)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to bind UDP receive socket: %s", std::strerror(errno));
            close(receiveSockfd);
            return;
        }
        while (rclcpp::ok()) {
            int bytesReceived = recvfrom(receiveSockfd, &cmd, sizeof(FCUCMD), 0, reinterpret_cast<sockaddr*>(&receiveAddr), &addrLen);

            if (bytesReceived > 0) {
                if (bytesReceived == sizeof(FCUCMD)) {
                    RCLCPP_INFO(this->get_logger(), "Received FCUCMD: Setting Offboard");
                    if (!offboard_set) {
                        offboard_set = setOffboardMode();
                    }
                    std::lock_guard<std::mutex> lock(udp_mutex);
                    geographic_msgs::msg::GeoPoseStamped targetPose;
                    targetPose.header.stamp = this->now();
                    targetPose.pose.position.latitude = cmd.RefLatitude_deg;
                    targetPose.pose.position.longitude = cmd.RefLongitude_deg;
                    targetPose.pose.position.altitude = cmd.RefHeight_meter;

                    pose_publisher_->publish(targetPose);
                    RCLCPP_INFO(this->get_logger(), "Set success");
                } else {
                    RCLCPP_ERROR(this->get_logger(), "Incomplete FCUCMD data packet received");
                }
            } else if (bytesReceived < 0) {
                RCLCPP_ERROR(this->get_logger(), "UDP Receive error: %s", std::strerror(errno));
            }
        }
    }
    void setup_udp_communication() {
        sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd_ < 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to create UDP socket");
            return;
        }
        
        memset(&destAddr_, 0, sizeof(destAddr_));
        destAddr_.sin_family = AF_INET;
        destAddr_.sin_port = htons(UDP_PORT);
        inet_pton(AF_INET, UDP_TARGET_IP, &destAddr_.sin_addr);
    }

    void start_udp_send_thread() {
        send_thread_ = std::thread(&UavUdpComm::udp_send_thread, this);
    }

    void udp_send_thread() {
        while (rclcpp::ok()) {

            send_udp_data();
            std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 发送间隔
        }
    }

    void send_udp_data() {
        
        std::lock_guard<std::mutex> lock(data_mutex_);
        ssize_t bytesSent = sendto(sockfd_, &data, sizeof(data), 0,
                                   (struct sockaddr*)&destAddr_, sizeof(destAddr_));
        if (bytesSent < 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to send UDP data: %s", strerror(errno));
        } else {
            RCLCPP_INFO(this->get_logger(), "Successfully sent UDP data");
        }
    }

    int sockfd_;
    struct sockaddr_in destAddr_;
    std::mutex data_mutex_;


    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
    rclcpp::Publisher<geographic_msgs::msg::GeoPoseStamped>::SharedPtr pose_publisher_;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;


    std::thread send_thread_;
    std::thread receive_thread_;
    uav_pos_raw data;
    std::mutex data_mutex;
    uint8_t seq;
    // UDP socket variables
    int sockfd, receiveSockfd;
    struct sockaddr_in destAddr, receiveAddr;
    socklen_t addrLen = sizeof(receiveAddr);

};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UavUdpComm>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
